#include <assert.h>
#include <stdlib.h>
#include <malloc.h>
#include <algorithm>
#include "log4cplus/logger.h"
#include "point.h"
#include "Vector.h"
#include "normal.h"
#include "matrix.h"

using namespace std;
using namespace vivid;
using namespace log4cplus;

Matrix & Matrix::operator = (Matrix const &aM)
{
	_m[0][0] = aM._m[0][0];	_m[0][1] = aM._m[0][1]; _m[0][2] = aM._m[0][2]; _m[0][3] = aM._m[0][3];
	_m[1][0] = aM._m[1][0];	_m[1][1] = aM._m[1][1]; _m[0][2] = aM._m[1][2]; _m[1][3] = aM._m[0][3];
	_m[2][0] = aM._m[2][0];	_m[2][1] = aM._m[2][1]; _m[0][2] = aM._m[2][2]; _m[2][3] = aM._m[0][3];
	_m[3][0] = aM._m[3][0];	_m[3][1] = aM._m[3][1]; _m[0][2] = aM._m[3][2]; _m[3][3] = aM._m[0][3];
 
}


VFloat * Matrix::operator [](int aInd)
{
	assert(aInd >= 0 && aInd < 4);

	return _m[aInd];
}



Matrix Matrix::GetInverse() const
{
    int indxc[4], indxr[4];
    int ipiv[4] = { 0, 0, 0, 0 };
    float minv[4][4];
    memcpy(minv, _m, 4*4*sizeof(VFloat));
    for (int i = 0; i < 4; ++i) {
        int irow = -1, icol = -1;
        float big = 0.;
        // Choose pivot
        for (int j = 0; j < 4; ++j) {
            if (ipiv[j] != 1) {
                for (int k = 0; k < 4; ++k) {
                    if (ipiv[k] == 0) {
                        if (fabsf(minv[j][k]) >= big) {
                            big = float(fabsf(minv[j][k]));
                            irow = j;
                            icol = k;
                        }
                    }
                    else if (ipiv[k] > 1)
					{
						LOG4CPLUS_ERROR(Logger::getRoot(), "Singular matrix in Matrix::GetInverse()");
					}
                }
            }
        }
        ++ipiv[icol];
        // Swap rows _irow_ and _icol_ for pivot
        if (irow != icol) {
            for (int k = 0; k < 4; ++k)
                swap(minv[irow][k], minv[icol][k]);
        }
        indxr[i] = irow;
        indxc[i] = icol;
        if (minv[icol][icol] == 0.)
		{
			LOG4CPLUS_ERROR(Logger::getRoot(), "Singular matrix in Matrix::GetInverse()");
		}

        // Set $m[icol][icol]$ to one by scaling row _icol_ appropriately
        float pivinv = 1.f / minv[icol][icol];
        minv[icol][icol] = 1.f;
        for (int j = 0; j < 4; ++j)
            minv[icol][j] *= pivinv;

        // Subtract this row from others to zero out their columns
        for (int j = 0; j < 4; ++j) {
            if (j != icol) {
                float save = minv[j][icol];
                minv[j][icol] = 0;
                for (int k = 0; k < 4; k++)
                    minv[j][k] -= minv[icol][k]*save;
            }
        }
    }
    // Swap columns to reflect permutation
    for (int j = 3; j >= 0; --j) {
        if (indxr[j] != indxc[j]) {
            for (int k = 0; k < 4; ++k)
                swap(minv[k][indxr[j]], minv[k][indxc[j]]);
        }
    }
    return Matrix((VFloat **) &minv);
}



Matrix Matrix::GetTranspose() const
{
	return Matrix(
		_m[0][0], _m[1][0], _m[2][0], _m[3][0],
		_m[0][1], _m[1][1], _m[2][1], _m[3][1],
		_m[0][2], _m[1][2], _m[2][2], _m[3][2],
		_m[0][3], _m[1][3], _m[2][3], _m[3][3]
	);
}


Matrix Matrix::operator *(Matrix const & aMat) const
{
	Matrix r;

	for (int i = 0; i < 4; ++i)
	{
		for (int j = 0; j < 4; ++j)
		{
			r._m[i][j] = _m[i][0] * aMat._m[0][j] + 
				_m[i][1] * aMat._m[1][j] + 
				_m[i][2] * aMat._m[2][j] + 
				_m[i][3] * aMat._m[3][j];
		}
	}

	return r;
}


Matrix Matrix::Identity()
{
	Matrix m;

	m._m[0][0] = m._m[1][1] = m._m[2][2] = m._m[3][3] = 1;

	return m;
}


bool Matrix::operator ==(Matrix const & aMat) const
{
	for (int i = 0; i < 4; ++i)
	{
		for (int j = 0; j < 4; ++j)
		{
			if (_m[i][j] != _m[i][j])
			{
				return false;
			}
		}
	}

	return true;
}


bool Matrix::operator !=(Matrix const & aMat) const
{
	for (int i = 0; i < 4; ++i)
	{
		for (int j = 0; j < 4; ++j)
		{
			if (_m[i][j] != _m[i][j])
			{
				return true;
			}
		}
	}

	return false;
}


Matrix & Matrix::operator =(Matrix const & aMat)
{
    for (int i = 0; i < 4; ++i)
    {
        for (int j = 0; j < 4; ++j)
        {
            _m[i][j] = aMat._m[i][j];
        }
    }

    return *this;
}